/*
 * Copyright (c)  Inc.
 * Author: geremy yang
 * This node is used to read canet port N data and publish the data content, and
 * subscription data content into canet port N.
 * * *************************************************************************
 * */
#ifndef XZACTUATORNODE_H
#define XZACTUALORNODE_H

#include "pthread.h"
#include "xfultrasonicdriver/ultrasonic_msg.h"
#include "nlink_parser/LinktrackAoaNodeframe0.h"
#include "xfcanetdriver/controlresult.h"
#include "uwbautofollow/pid.h"
#include "uwbautofollow/psd.h"
#include "uwbautofollow/obstacle_grid.h"
#include "uwbautofollow/dead_zone.h"

#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <string.h>
#include <ros/time.h>
#include <ros/duration.h>
#include <geometry_msgs/Twist.h>
#include <cmath>

// #include <windows.h>

using namespace std;

class UwbAutoFollow
{
public:
  UwbAutoFollow(ros::NodeHandle &handle);
  ~UwbAutoFollow();
  void run();

private:
  void follow_me();
  void callback_ultrasonic(const xfultrasonicdriver::ultrasonic_msg::ConstPtr &msg);
  void callback_uwb(const nlink_parser::LinktrackAoaNodeframe0::ConstPtr &msg);
  void callback_laserscan(const sensor_msgs::LaserScan::ConstPtr &msg);

  void AOA_Application();
  void Move_Control(int16_t speed, int16_t turn_angle); // 运动控制

  void debug_out(int debug_id);

//for test
  void Publish_obstacle_grid();
  void Publish_obstacle_angle();

public:
  bool bdebug;
  string debug_str;

  ros::NodeHandle m_handle;
  // interface
  ros::Publisher pub_control;

  ros::Publisher pub_test_obstacles_raw;
  ros::Publisher pub_test_obs_angle_inflation;

  ros::Subscriber sub_uwb;
  ros::Subscriber sub_ultrasonic;
  ros::Subscriber sub_laserscan;

  // data
  xfultrasonicdriver::ultrasonic_msg ultrasonic_info;
  //ParkSensorDistance m_psd;
  ObstacleGrid m_obs_grid;

  int lindar_rx_timeout = 0;
  int uwb_rx_timeout = 0;


  const float angle_target_range = 2.0;    // 双环电机消抖中目标环范围，角度绝对值小于目标环时进入死区环判断
  const float angle_dead_zone_range = 9.0; // 双环电机消抖中死区环范围，角度绝对值小于死区环时输出为0
  uint8_t deadzone_angle_range_flag = 0;  // 进入死区环标志位

  float angle_measure_uwb = 0.0;      // 角度测量值
  const float angle_expect = 0.0; // 零偏值，角度跟随期望值

  float dis_measure_uwb = 0.0; // 距离测量值
  float dis_expect = 2.5;  // 区间距离跟随暂时不采用PID，该值为区间中心，单位：m
  float dis_range = 0.5;   // 区间距离跟随的区间大小，单位：m

  const int y1_output = 0; // 前进后退输出量，范围[-100，100]
  int y1_increase = 0;     // 前进后退输出增量，范围[0，100]，决定前进后退的速度
  const int z1_output = 0; // 旋转输出量，范围[-90，90]
  int z1_increase = 0;     // 旋转输出增量，范围[0，127]，决定旋转的速度

  bool avoid_danger_switch = true; // 避障模式开关，0关1开
  uint8_t avoid_danger_status = 0; // 避障模式状态，1表示处于避障算法控制中

  uint8_t deadzone_obs_stop_flag = 0; // 进入死区环标志位
  float stop_distance = 1.0;        // 避障距离阈值,单位：米
  float danger_distance = 1.5;      // 避障距离阈值,单位：米
  float side_danger_distance = 1.0; // 避障距离阈值,单位：米
  float slow_down_distance = 2.0;   // 避障减速缓冲区距离阈值,单位：米
  int avoid_danger_turn_angle = 30; // 避障开环控制横移速度，范围[0,100]

  int follow_me_speed = 20;              // 初始跟随速度，范围[0,100]
  int avoid_danger_slow_down_speed = 10; // 避障减速缓冲区减速速度，范围[0,100]
  int MAX_SPEED = 20;
  uint8_t deadzone_distance_range_flag = 0; // 进入死区环标志位

  int left_right_obstacle_Duration = 1000;
  int left_obstacle_timeout = 1000;
  int right_obstacle_timeout = 1000;
};

#endif
